import math
from queue import PriorityQueue

directions = [(1, 0), (0, 1), (-1, 0), (0, -1)]


class Node:
    def __init__(self, x, y, parent=None):
        self.x = x
        self.y = y
        self.parent = parent
        self.cost = 0
        self.heuristic = 0
        self.priority = 0  # 值越小，优先级越大

    def __eq__(self, other):
        return self.x == other.x and self.y == other.y

    def __lt__(self, other):
        return self.priority < other.priority

    def __hash__(self):
        return hash((self.x, self.y))

    @staticmethod
    def Manhattan(a, b):
        """如果只允许朝上下左右四个方向运动，使用曼哈顿距离"""
        return abs(a.x - b.x) + abs(a.y - b.y)

    @staticmethod
    def DiagnalD(a, b):
        """如果允许沿米字八个方向运动，使用对角距离"""
        dx = abs(a.x - b.x)
        dy = abs(a.y - b.y)
        return (math.sqrt(2) - 2) * min(dx, dy) + dx + dy

    @staticmethod
    def Euclidean(a, b):
        """如果允许朝任何方向移动，使用欧几里得距离"""
        return ((a.x - b.x) ** 2 + (a.y - b.y) ** 2) ** 0.5

    def CalcPriority(self, cur, ends):
        self.cost = cur.cost + 1
        values = [Node.Manhattan(self, end) for end in ends]
        self.heuristic = min(values)
        self.priority = self.cost + self.heuristic


def BuildPath(node):
    path = []
    while node is not None:
        path.append((node.x, node.y))
        node = node.parent
    return path


def Astar(map, start, goals):
    openList = PriorityQueue()
    rows = len(map)
    if rows == 0:
        return []
    cols = len(map[0])
    closeList = [[False] * cols for _ in range(rows)]
    ends = []
    for goal in goals:
        if (
            goal.x >= 0
            and goal.x < rows
            and goal.y >= 0
            and goal.y < cols
            and not map[goal.x][goal.y]
        ):
            ends.append(goal)
    if (0 <= start.x < rows and 0 <= start.y < cols):
        openList.put(start)
        closeList[start.x][start.y] = True

    while not openList.empty():
        currentNode = openList.get()

        if currentNode in ends:
            return BuildPath(currentNode)
        
        for i in range(4):
            newX = currentNode.x + directions[i][0]
            newY = currentNode.y + directions[i][1]

            if (
                newX >= 0
                and newX < rows
                and newY >= 0
                and newY < cols
                and not closeList[newX][newY]
                and not map[newX][newY]
            ):
                newNode = Node(newX, newY, currentNode)
                newNode.CalcPriority(currentNode, ends)
                openList.put(newNode)
                closeList[newX][newY] = True
    return []


if __name__ == "__main__":
    import random
    import sys

    def main():
        # 生成一个rows x cols的地图，其中0表示可走路径，1表示障碍物
        def RandomMap(rows, cols, obstacle_rate):
            map = [
                [0 if random.random() > obstacle_rate else 1 for _ in range(cols)]
                for _ in range(rows)
            ]
            return map

        def int_input(message):
            while True:
                try:
                    return int(input(message))
                except ValueError:
                    print("请输入一个整数。")

        rows = int_input("请输入地图的行数：")
        if rows == 0:
            return 1
        cols = int_input("请输入地图的列数：")
        if cols == 0:
            return 1

        map = RandomMap(rows, cols, 0.2)

        start_x = int_input("请输入起点坐标x：")
        start_y = int_input("请输入起点坐标y：")
        start = Node(start_x, start_y)
        if start.x >= 0 and start.x < rows and start.y >= 0 and start.y < cols:
            map[start.x][start.y] = 0
        else:
            return 1

        goals = []
        goals.append(Node(-1, -1))
        goals.append(Node(5, 5))
        goals.append(Node(3, 7))
        goals.append(Node(7, 3))

        for node in goals:
            if node.x >= 0 and node.x < rows and node.y >= 0 and node.y < cols:
                map[node.x][node.y] = 0

        path = Astar(map, start, goals)

        for point in path:
            if map[point[0]][point[1]]:
                print("Path crash obstacle!")
                return 1
            map[point[0]][point[1]] = -1

        map[start.x][start.y] = 2
        for node in goals:
            if node.x >= 0 and node.x < rows and node.y >= 0 and node.y < cols:
                map[node.x][node.y] = 3

        for row in map:
            for cell in row:
                if cell == -1:
                    print("* ", end="")
                elif cell == 1:
                    print("# ", end="")
                elif cell == 2:
                    print("S ", end="")
                elif cell == 3:
                    print("E ", end="")
                else:
                    print("  ", end="")
            print()

        return 0

    sys.exit(main())
